#ifndef VENUS_GEOMETRY_TRANSFORM_H
#define VENUS_GEOMETRY_TRANSFORM_H

#include "common/venus.h"
#include "ray.h"
#include "point.h"
#include "vector3.h"
#include "normal.h"
#include "aabb.h"
#include "matrix4x4.h"

namespace venus {
class Transform {
public:
  Transform() { }
  Transform(const float mat[4][4]) {
    m = Matrix4x4(mat[0][0], mat[0][1], mat[0][2], mat[0][3],
                  mat[1][0], mat[1][1], mat[1][2], mat[1][3],
                  mat[2][0], mat[2][1], mat[2][2], mat[2][3],
                  mat[3][0], mat[3][1], mat[3][2], mat[3][3]);
    mInv = Inverse(m);
  }
  Transform(const Matrix4x4 &mat)
      : m(mat), mInv(Inverse(mat)) {
  }
  Transform(const Matrix4x4 &mat, const Matrix4x4 &minv)
     : m(mat), mInv(minv) {
  }
  void Print(FILE *f) const;
  friend Transform Inverse(const Transform &t) {
    return Transform(t.mInv, t.m);
  }
  friend Transform Transpose(const Transform &t) {
    return Transform(Transpose(t.m), Transpose(t.mInv));
  }
  bool operator==(const Transform &t) const { 
    return t.m == m && t.mInv == mInv;
  }
  bool operator!=(const Transform &t) const {
    return t.m != m || t.mInv != mInv;
  }
  bool operator<(const Transform &t2) const {
    for (uint32_t i = 0; i < 4; ++i)
      for (uint32_t j = 0; j < 4; ++j) {
        if (m[i][j] < t2.m[i][j]) return true;
        if (m[i][j] > t2.m[i][j]) return false;
      }
    return false;
  }
  bool IsIdentity() const {
    return (m[0][0] == 1.f && m[0][1] == 0.f &&
            m[0][2] == 0.f && m[0][3] == 0.f &&
            m[1][0] == 0.f && m[1][1] == 1.f &&
            m[1][2] == 0.f && m[1][3] == 0.f &&
            m[2][0] == 0.f && m[2][1] == 0.f &&
            m[2][2] == 1.f && m[2][3] == 0.f &&
            m[3][0] == 0.f && m[3][1] == 0.f &&
            m[3][2] == 0.f && m[3][3] == 1.f);
  }
  const Matrix4x4 &GetMatrix() const { return m; }
  const Matrix4x4 &GetInverseMatrix() const { return mInv; }
  bool HasScale() const {
    float la2 = this->Do(Vector3(1,0,0)).LengthSquared();
    float lb2 = this->Do(Vector3(0,1,0)).LengthSquared();
    float lc2 = this->Do(Vector3(0,0,1)).LengthSquared();
#define NOT_ONE(x) ((x) < .999f || (x) > 1.001f)
    return (NOT_ONE(la2) || NOT_ONE(lb2) || NOT_ONE(lc2));
#undef NOT_ONE
  }
  inline Point Do(const Point &pt) const;
  inline void Do(const Point &pt, Point *ptrans) const;
  inline Vector3 Do(const Vector3 &v) const;
  inline void Do(const Vector3 &v, Vector3 *vt) const;
  inline Normal Do(const Normal &) const;
  inline void Do(const Normal &, Normal *nt) const;
  AABB Do(const AABB &b) const;
  Transform operator*(const Transform &t2) const;
  bool SwapsHandedness() const;
private:
  // Transform Private Data
  Matrix4x4 m, mInv;
  friend class AnimatedTransform;
  friend struct Quaternion;
};

Transform Translate(const Vector3 &delta);
Transform Scale(float x, float y, float z);
Transform RotateX(float angle);
Transform RotateY(float angle);
Transform RotateZ(float angle);
Transform Rotate(float angle, const Vector3 &axis);
Transform LookAt(const Point &pos, const Point &look, const Vector3 &up);
Transform Orthographic(float znear, float zfar);
Transform Perspective(float fov, float znear, float zfar);

// Transform Inline Functions
inline Point Transform::Do(const Point &pt) const {
  float x = pt.x, y = pt.y, z = pt.z;
  float xp = m[0][0]*x + m[0][1]*y + m[0][2]*z + m[0][3];
  float yp = m[1][0]*x + m[1][1]*y + m[1][2]*z + m[1][3];
  float zp = m[2][0]*x + m[2][1]*y + m[2][2]*z + m[2][3];
  float wp = m[3][0]*x + m[3][1]*y + m[3][2]*z + m[3][3];
  assert(wp != 0);
  if (wp == 1.) return Point(xp, yp, zp);
  else          return Point(xp, yp, zp)/wp;
}


void Transform::Do(const Point &pt, Point *ptrans) const {
  float x = pt.x, y = pt.y, z = pt.z;
  ptrans->x = m[0][0]*x + m[0][1]*y + m[0][2]*z + m[0][3];
  ptrans->y = m[1][0]*x + m[1][1]*y + m[1][2]*z + m[1][3];
  ptrans->z = m[2][0]*x + m[2][1]*y + m[2][2]*z + m[2][3];
  float w   = m[3][0]*x + m[3][1]*y + m[3][2]*z + m[3][3];
  if (w != 1.) *ptrans /= w;
}

inline Vector3 Transform::Do(const Vector3 &v) const {
  float x = v.x, y = v.y, z = v.z;
  return Vector3(m[0][0]*x + m[0][1]*y + m[0][2]*z, 
                 m[1][0]*x + m[1][1]*y + m[1][2]*z, 
                 m[2][0]*x + m[2][1]*y + m[2][2]*z);
}

inline void Transform::Do(const Vector3 &v, Vector3 *vt) const {
  float x = v.x, y = v.y, z = v.z;
  vt->x = m[0][0] * x + m[0][1] * y + m[0][2] * z;
  vt->y = m[1][0] * x + m[1][1] * y + m[1][2] * z;
  vt->z = m[2][0] * x + m[2][1] * y + m[2][2] * z;
}

inline Normal Transform::Do(const Normal &n) const {
  float x = n.x, y = n.y, z = n.z;
  return Normal(mInv[0][0]*x + mInv[1][0]*y + mInv[2][0]*z,
                mInv[0][1]*x + mInv[1][1]*y + mInv[2][1]*z,
                mInv[0][2]*x + mInv[1][2]*y + mInv[2][2]*z);
}

inline void Transform::Do(const Normal &n, Normal *nt) const {
  float x = n.x, y = n.y, z = n.z;
  nt->x = mInv[0][0] * x + mInv[1][0] * y + mInv[2][0] * z;
  nt->y = mInv[0][1] * x + mInv[1][1] * y + mInv[2][1] * z;
  nt->z = mInv[0][2] * x + mInv[1][2] * y + mInv[2][2] * z;
}

} // namespace venus

#endif // VENUS_GEOMETRY_TRANSFORM_H
